﻿module VisualPhysics.Scene.Core.Operators

open System
open System.Drawing

let private compressAngle a =
    let mutable a = a
    while a >= (2.0 * Math.PI) do
        a <- a - (2.0 * Math.PI)
    while a < 0.0 do
        a <- a + (2.0 * Math.PI)
    a

/// <summary>A corrected version of arcsine.</summary>
/// <param name="A">First side.</param>
/// <param name="B">Second side.</param>
/// <returns>The correct angle.</returns>
let inline asin2 a b =
    let a = float a
    let b = float b
    match sign a, sign b with
    | 1, 1
    | 1, -1 -> asin (a / b)
    | -1, 1
    | -1, -1 -> Math.PI + (asin (a / b))
    | 0, _ -> 0.0
    | _, 0 -> Math.PI / 2.0
    | _ -> failwith "Unexpected!"

/// <summary>A corrected version of arccosine.</summary>
/// <param name="A">First side.</param>
/// <param name="B">Second side.</param>
/// <returns>The correct angle.</returns>
let inline acos2 a b =
    let a = float a
    let b = float b
    match sign a, sign b with
    | 1, 1
    | -1, 1 -> acos (a / b)
    | 1, -1
    | -1, -1 -> 2.0 * Math.PI - (acos (a / b))
    | 0, _ -> Math.PI / 2.0
    | _, 0 -> 0.0
    | _ -> failwith "Unexpected!"

/// <summary>A corrected version of arctangent.</summary>
/// <param name="A">First side.</param>
/// <param name="B">Second side.</param>
/// <returns>The correct angle.</returns>
let inline atan2 a b =
    let a = float a
    let b = float b
    match sign a, sign b with
    | 1, 1
    | 1, -1 -> atan (a / b)
    | -1, 1
    | -1, -1 -> Math.PI + (atan (a / b))
    | 0, _ -> 0.0
    | _, 0 -> Math.PI / 2.0
    | _ -> failwith "Unexpected!"

/// <summary>Converts a numeric tuple to a System.Drawing.PointF</summary>
/// <param name="X">The X part of the PointF</param>
/// <param name="Y">The Y part of the PointF</param>
/// <returns>The PointF</returns>
let inline point (x, y) =
    PointF(float32 x, float32 y)

/// <summary>Converts a float from degrees to radians.</summary>
/// <param name="Radians">The angle in degrees.</param>
/// <returns>The angle in radians.</returns>
let deg2rad : (float<degree> -> float) = ((*) (1.0 / 180.0<degree>)) >> ((*) Math.PI)

/// <summary>Converts a float from radians to degrees.</summary>
/// <param name="Radians">The angle in radians.</param>
/// <returns>The angle in degrees.</returns>
let rad2deg : (float -> float<degree>) = ((*) 180.0<degree>) >> ((*) (1.0 / Math.PI))

/// <summary>Calculates the distance between two points.</summary>
/// <param name="A">The first point.</param>
/// <param name="B">The second point.</param>
/// <returns>The distance between the points.</returns>
let distance (a : (float<_> * float<_>)) (b : (float<_> * float<_>)) =
    (fst b - fst a) * (fst b - fst a) + (snd b - snd a) * (snd b - snd a)
    |> sqrt

/// <summary>Active pattern to get the value of an option.</summary>
/// <param name="Option">The option.</param>
/// <returns>The value of the option.</returns>
let (|ValueOnly|) option =
    match option with
    | Some r -> r
    | None -> raise (invalidArg "option" "Cannot be none!")

/// <summary>Converts Cartesian vector to polar vector.</summary>
/// <param name="Vector">The Cartesian vector.</param>
/// <returns>The polar vector.</returns>
let pol (vector : (float<_> * float<_>)) : (MVector<_>) =
    let x, y = vector
    let length = sqrt (x * x + y * y)
    let direction = atan2 y x
    match sign x, sign y with
    | 0, 1
    | 1, 0
    | 1, 1 ->
        (length, compressAngle direction)
    | 0, -1
    | -1, 0
    | 1, -1 ->
        (length, compressAngle (2.0 * Math.PI - direction))
    | -1, 1 ->
        (length, compressAngle (Math.PI - direction))
    | -1, -1 ->
        (length, compressAngle (Math.PI + direction))
    | 0, 0 -> (0.0<_>, 0.0)
    | _ -> failwith "Unexpected!"
        

/// <summary>Converts polar vector to Cartesian vector.</summary>
/// <param name="Vector">The polar vector.</param>
/// <returns>The Cartesian vector.</returns>
let inline car (vector : MVector<'u>) : (float<'u> * float<'u>) =
    let length, direction = vector
    let x = length * cos direction
    let y = length * sin direction
    (x, y)

/// <summary>Rotates a vector.</summary>
/// <param name="Vector">The vector.</param>
/// <param name="Teta">The angle by which the vector will be rotated.</param>
/// <returns>The rotated vector.</returns>
let rotate (vector : (float<_> * float<_>)) (teta : float) =
    let length, direction = pol vector
    car (length, direction + teta)